/* Copyright (c) 2011 Alexandre Défossez, Timothée Lacroix
 * This file is part of HINAPT.
 *
 * HINAPT is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * HINAPT is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with HINAPT.  If not, see <http://www.gnu.org/licenses/>.
 */


#include "utils.hpp"

void Assert(bool condition, string s){
  if (!condition)
    throw std::runtime_error(s);
  return;
}

void cumulate(Mat &img){
  Mat copie=img.clone();
  int width=img.cols;
  int height=img.rows;
  img.create(height,width,CV_32SC1);
  img.at<int>(0,0)=copie.at<uchar>(0,0);
  //puis on accumule
  for(int y=0;y<height;y++){
      for(int x=0;x<width;x++){
          if(x==0){
              if(y!=0)
                  img.at<int>(y,x)=img.at<int>(y-1,x)+copie.at<uchar>(y,x);
          }
          else{
              if(y==0)
                  img.at<int>(y,x)=img.at<int>(y,x-1)+copie.at<uchar>(y,x);
              else
                  img.at<int>(y,x)=img.at<int>(y,x-1)+img.at<int>(y-1,x)-
                  img.at<int>(y-1,x-1)+copie.at<uchar>(y,x);
          }
      }
  }
}

void MatToSFML(Mat& source, sf::Image &res){
  using namespace sf;
  int channels = source.channels();
  Color color;
  res.Create(source.cols, source.rows);
  for (int x=0; x<source.cols; x++){
    for (int y=0; y<source.rows; y++){
      color.r = 0;
      color.g = 0;
      color.b = 0;
      color.a = 255;
      switch (channels){
        case 1://niveau de gris vers RGBA
          color.b = source.at<uchar>(y,x);
          color.g = source.at<uchar>(y,x);
          color.r = source.at<uchar>(y,x);
          break;
        case 3://BGR vers RGBA
          color.b = source.at<Vec3b>(y,x)[0];
          color.g = source.at<Vec3b>(y,x)[1];
          color.r = source.at<Vec3b>(y,x)[2];
          break;
        case 4://BGRA vers RGBA
          color.b = source.at<Vec4b>(y,x)[0];
          color.g = source.at<Vec4b>(y,x)[1];
          color.r = source.at<Vec4b>(y,x)[2];
          color.a = source.at<Vec4b>(y,x)[3];
          break;
        default:
          break;
      }
      res.SetPixel(x,y, color);
    }
  }
}

void normalize(Mat &source){
  toGrayScale(source);
  Mat buf;
  flip(source, buf, 1);
  source = buf;
  equalizeHist(source, buf);
  source = buf;
}

void toGrayScale(Mat &img){
  Mat gray;
  cvtColor(img, gray, CV_BGR2GRAY);
  img = gray;
}

void pyramidize(Mat &prev,Mat &next){
  Size size = prev.size();
  int height = size.height/2;
  int width = size.width/2;
  next.create(height, width, CV_8UC1);
  #pragma omp parallel for
  for (int y=0; y<height; y++){
    for (int x=0; x<width; x++){
      next.at<uchar>(y,x)=prev.at<uchar>(2*y,2*x)/4 +
                         (prev.at<uchar>(2*y-1,2*x)+
                            prev.at<uchar>(2*y+1,2*x)+
                            prev.at<uchar>(2*y,2*x-1)+
                            prev.at<uchar>(2*y,2*x+1))/8 +
                         (prev.at<uchar>(2*y-1,2*x-1)+
                            prev.at<uchar>(2*y+1,2*x+1)+
                            prev.at<uchar>(2*y+1,2*x-1)+
                            prev.at<uchar>(2*y-1,2*x+1))/16;
    }
  }
}


void extract(Mat& a,Mat &b,Point2f center,int size, bool clone){
  Rect roi(center.x-size/2, center.y-size/2, size, size);
  if (clone)
    b = Mat(a, roi).clone();
  else
    b = Mat(a, roi);
}

Mat extract(Mat& a,Point2f center,int size, bool clone){
  Mat b;
  extract(a, b, center, size, clone);
  return b;
}

void getImage(VideoCapture &capture, Mat &img){
  capture.retrieve(img);
  capture.grab();
  Assert(!img.empty(), errors::camera_failure);
  normalize(img);
}


Point2f center(Rect r){
  return Point2f(r.x + r.width/2., r.y + r.height/2.);
}

bool inbound(Point2f &p, Rect boundaries){
  Point tmp(p.x, p.y);

  if (boundaries.contains(tmp))
    return true;
  tmp.x = max(boundaries.x, tmp.x);
  tmp.y = max(boundaries.y, tmp.y);
  tmp.x = min(boundaries.br().x-1, tmp.x);
  tmp.y = min(boundaries.br().y-1, tmp.y);
  p.x = tmp.x;
  p.y = tmp.y;
  return false;
}

bool acceptChange(double delta,double deltaMax,double param){
  static boost::minstd_rand gen(time(NULL));
  static boost::uniform_real<double> distrib(0, 1);
  double proba=exp(delta*param);
  double random=distrib(gen);
  return random>proba;
}

void pprintMat(Mat &m){
  printf("------------ MATRIX DUMP BEGIN -----------\n");
  for(int y=0; y<m.rows; y++){
    for(int x=0; x<m.cols; x++){
      for(int i=0; i<m.channels(); i++)
        printf("%d ", (&m.at<int>(y, x))[i]);
      printf("| ");
    }
    printf("\n");
  }
  printf("-------- MATRIX DUMP END -----------\n");

}

int evalFeature(double _x1,double _y1,double _x2,double _y2,
                int feature,Mat img){
  int x1=int((_x1/((double)nbTranche-1.))*double(img.cols-1));
  int x2=int((_x2/((double)nbTranche-1.))*double(img.cols-1));
  int y1=int((_y1/((double)nbTranche-1.))*double(img.rows-1));
  int y2=int((_y2/((double)nbTranche-1.))*double(img.rows-1));

  int width=x2-x1;
  int height=y2-y1;
  switch(feature){
    case 0:
      return evalRect(x1,y1,x1+width/2,y2,img)-
             evalRect(x1+width/2+1,y1,x2,y2,img);
    break;
    case 1:
      return evalRect(x1,y1,x2,y1+height/2,img)-
             evalRect(x1,y1+height/2,x2,y2,img);
    break;
    case 2:
      return evalRect(x1,y1,x1+width/3,y2,img)-
             evalRect(x1+width/3+1,y1,x1+2*width/3,y2,img)+
             evalRect(x1+2*width/3+1,y1,x2,y2,img);
    break;
    case 3:
      return evalRect(x1,y1,x1+width/2,y1+height/2,img)-
             evalRect(x1+width/2+1,y1,x2,y1+height/2,img)-
             evalRect(x1,y1+height/2+1,x1+width/2,y2,img)+
             evalRect(x1+width/2+1,y1+height/2+1,x2,y2,img);
    break;
  }
  return 0;
}


int evalRect(int x1,int y1,int x2,int y2,Mat img){
  if(y1>0 && x1>0)
    return img.at<int>(x2,y2)    -
           img.at<int>(x2,y1-1)  -
           img.at<int>(x1-1,y2)  +
           img.at<int>(x1-1,y1-1);
  else if(y1>0 && x1==0)
    return img.at<int>(x2,y2)    -
           img.at<int>(x2,y1-1);
  else if(y1==0 && x1>0)
    return img.at<int>(x2,y2)    -
           img.at<int>(x1-1,y2);
  else
    return img.at<int>(x2,y2);
}

void normalize(vector<double> &weights){
  double total=0;
  for(int i=0;i<(int)weights.size();i++)
    total+=weights[i];
  for(int i=0;i<(int)weights.size();i++)
    weights[i]=weights[i]/total;
}
